#include <joyos.h>
#include <Math.h>
#include "navigate5.h"
#include "gather.h"
#include "deposit.h"

//measurements
#define VPS_PER_FOOT 443.4


//points to dump from for the red Team
Point teamRedTarget[3] = {
	{(221.5)/VPS_PER_FOOT, (384)/VPS_PER_FOOT},
	{(-221.5)/VPS_PER_FOOT, (384)/VPS_PER_FOOT},
	{(-443)/VPS_PER_FOOT, 0},
};


//points to dump from for the blue Team
Point teamBlueTarget[3] = {
	{(-221.5)/VPS_PER_FOOT, (-384)/VPS_PER_FOOT},
	{(221.5)/VPS_PER_FOOT, (-384)/VPS_PER_FOOT},
	{(443)/VPS_PER_FOOT, 0},
};


Point dumpTurnPoints[6] = {
	{(2047)/VPS_PER_FOOT, 0},
	{(1024)/VPS_PER_FOOT, (1773)/VPS_PER_FOOT},
	{(-1024)/VPS_PER_FOOT, (1773)/VPS_PER_FOOT},
	{(-2047)/VPS_PER_FOOT, 0},
	{(-1024)/VPS_PER_FOOT, (-1773)/VPS_PER_FOOT},
	{(1024)/VPS_PER_FOOT, (-1773)/VPS_PER_FOOT},
	
};


Point chooseDumpTarget(){ // could also take in team color as input... hrmm
	Point closestTarget;
	Point me = getMyPosition();
	if (getMyColor() == 1){
		closestTarget = teamBlueTarget[0];
		for(uint8_t i = 0; i < 3; i++){
			if(calculateDistance(closestTarget, me ) >= calculateDistance(teamBlueTarget[i], me)){
				closestTarget = teamBlueTarget[i];
			}
		}
	}else if (getMyColor() == 0){
		closestTarget = teamRedTarget[0];
		for(uint8_t i = 0; i < 3; i++){
			if(calculateDistance(closestTarget, me)>= calculateDistance(teamRedTarget[i], me)){
				closestTarget = teamRedTarget[i];
			}
		}
	}
	
	return closestTarget;
}




void depositSheep(){
	//drive to Point chosen by chooseDumpTarget, maybe not quite at the wall?
	//Turn to a specific heading
	//Backup (driveFeet with front reversed?) <-- need to then make switchFront() a public function
	//keep drive Feet with small incraments while !(swithch1 && switch2)
	//set_servo_pos( port#, 505?) ?= not quite the edge of the servo's range, but close
	//wait for a bit
	//set_servo_pos(port#, 5) -- I think the port # is 0? or 1, whichever is the lowest # existing
	
	
	uint8_t backUp = 1;
	Point dumpTarget = chooseDumpTarget();
	uint8_t dumpSection = inSection(dumpTarget);
	
	driveThroughFieldToPoint(getSection(dumpSection), 2./12.);
	turnToPoint(dumpTurnPoints[inSection(getMyPosition())-1]);
	
	//uint32_t start_time = get_time();
	
	
	driveForwardFeet( calculateDistance(getMyPosition(), dumpTarget) - 2./12., -150);	
	
	servo_set_pos(0, 500);
	pause(500);
	
	motor_set_vel(1, 200);
    motor_set_vel(0, -200);
	pause(100);
	motor_set_vel(1, 0);
    motor_set_vel(0, 0);
	pause(500);
    motor_set_vel(1, 200);
    motor_set_vel(0, -200);
	pause(100);
	motor_set_vel(1, 0);
    motor_set_vel(0, 0);
	pause(500);
	
	servo_set_pos(0, 50);
}